#include <pthread.h>
#include <vector>

#include "Game.h"
#include "GameObject.h"
#include "Unit.h"
#include "UnitType.h"
#include "../tools/AStar.h"
#include "../tools/Log.h"

using namespace std;

Unit::Unit(UnitType *Type, unsigned x, unsigned y) :
	ModelObject(), GameObject(), type(Type), path(), state(STOPPED)
{
	id = GameObject::get_next_id();
	health = Type->get_initial_health();		// Load initial health from corresponding unit type
	m = Type->get_model();
	
	game_position = lVec3D(x, y, Game::Instance()->getMap().getHeight(x, y));
}

void Unit::update(unsigned long dt)
{
	pthread_mutex_lock(&lock);
	
		if (state == MOVING && !path.empty())
			game_position = unit_position_in(dt);
		
		time_since_last_update = 0;
	
	pthread_mutex_unlock(&lock);
}

void Unit::move(const lVec2D& destination)
{
	lVec2D lockedDest(destination);
	lockedDest /= SUBSPACE_RESOLUTION;
	lockedDest *= SUBSPACE_RESOLUTION;
	
	lVec2D gP(game_position);
	gP /= SUBSPACE_RESOLUTION;
	gP *= SUBSPACE_RESOLUTION;
	
	path = a_star(Game::Instance()->getMap(), gP, lockedDest);
	if(!path.empty()) {
		path.erase(path.begin());
		path.push_back(destination);
	}
	
	// Hack for showing A* waypoints
	for(vector<lVec2D>::iterator itr = path.begin(); itr != path.end(); itr++)
	{
		Game* g = Game::Instance();
		new DebugDrawSpot(itr->x, itr->y, g->getMap().getHeight(itr->x, itr->y), 1, 1, 1, SUBSPACE_RESOLUTION / 50);
	}
	
	color.g = color.b = 0;
	state = MOVING;
}

lVec3D Unit::unit_position_in(unsigned long& dt)
{
	unsigned long dt_left = dt;
	lVec3D tmp_position = game_position;
	
	while (dt_left > 0 && !path.empty())
	{
		lVec3D path0(path[0].x, path[0].y, Game::Instance()->getMap().getHeight(path[0].x, path[0].y));
		unsigned long distance_to_waypoint = (path0 - tmp_position).magnitude();
		unsigned long time_to_waypoint = distance_to_waypoint / type->get_speed();
		
		lVec3D direction = path0 - tmp_position;
		
		// Compute draw orientation (converting from radians to degrees)
		orientation.z = std::atan2(direction.y, direction.x) * 180 / (3.14159);
		
		// We can't make it to the next waypoint. Return where we'll be when time runs out.
		if (time_to_waypoint > dt_left)
			return tmp_position + ((direction * type->get_speed() * dt_left) / direction.magnitude());
		
		// We've made it to the next waypoint
		dt_left -= time_to_waypoint;
		tmp_position += (direction * type->get_speed() * time_to_waypoint) / direction.magnitude();
		path.erase(path.begin());
	}
	
	return tmp_position;
}

void Unit::stop()
{
	path.clear();
	state = STOPPED;
}

void Unit::draw(unsigned long dt)
{
	pthread_mutex_lock(&lock);
		time_since_last_update += dt;
	
		switch(state) {
			case MOVING: {
				drawPos = convert(unit_position_in(time_since_last_update));
				break;
			}
			case STOPPED: {
				drawPos = convert(game_position);
				break;
			}
		}
	pthread_mutex_unlock(&lock);
	
	drawPos.z = Game::Instance()->getMap().getHeight(drawPos.x, drawPos.y);
	
	fVec3D euler = Game::Instance()->getMap().getMapEulerAngles(drawPos.x, drawPos.y);
	m.draw(drawPos, orientation, euler, fVec3D());
}

void Unit::getBoundingBox(fVec2D& a, fVec2D& b, fVec2D &c, fVec2D &d) {
	m.calculateBoundingBox(drawPos, orientation.z, a, b, c, d);
}
